#include "FaceOrientation.h"
#include <cmath>
#include "support.h"

FaceOrientation::FaceOrientation(void) : Filter(5, 1)
{
	outputs[0] = cv::Mat(1, 3, CV_32FC1);
}


FaceOrientation::~FaceOrientation(void)
{
}

void FaceOrientation::Process()
{
	float *in0 = inputs[0]->ptr<float>(0);
	float *in1 = inputs[1]->ptr<float>(0);
	float *in2 = inputs[2]->ptr<float>(0);
	float *in3 = inputs[3]->ptr<float>(0);

	cv::Vec3f leftEye(in0[0], in0[1], in0[2]);
	cv::Vec3f rightEye(in1[0], in1[1], in1[2]);
	cv::Vec3f nose(in2[0], in2[1], in2[2]);
	cv::Vec3f mouth(in3[0], in3[1], in3[2]);

	cv::Vec3f center = (leftEye + rightEye) / 2;
	cv::Vec3f noseVector = nose - center;

	//rotation
	cv::Vec3f diff = rightEye - leftEye;
	float distance = sqrt(diff[0]*diff[0] + diff[1]*diff[1] + diff[2]*diff[2]);
	cv::Vec3f rotationAxis = (rightEye - leftEye) / distance;

	int angle = inputs[INPUT_ANGLE]->at<int>(0,0);

	float rotationAngle = (angle / 180.0) * acosf(0)*2;


	
	float c = cos(rotationAngle);
	float s = sin(rotationAngle);
	float ux = rotationAxis[0];
	float uy = rotationAxis[1];
	float uz = rotationAxis[2];

	cv::Mat rotationMatrix = (cv::Mat_<float>(3, 3) <<
		c + ux*ux * (1 - c),		ux*uy*(1-c)-uz*s,		ux*uz*(1-c)+uy*s,
		uy*ux*(1-c)+uz*s,			c+uy*uy*(1-c),			uy*uz*(1-c)-ux*s,
		uz*ux*(1-c)-uy*s,			uz*uy*(1-c)+ux*s,		c+uz*uz*(1-c)
		);
	
	cv::Mat vec = (cv::Mat_<float>(3,1) << noseVector[0], noseVector[1], noseVector[2]);
	cv::Mat faceOrientationVector = rotationMatrix * vec;

	float *orientation = faceOrientationVector.ptr<float>(0);

	outputs[0] = (cv::Mat_<float>(1, 3) << orientation[0], orientation[1], orientation[2]);
}